#include "../include/7Dof_Model_dyna.h"

#include "ros/ros.h"
#include "../include/serial_struct.h"
#include "../include/hardware/robot.h"

#include <math.h>
#include <iostream>
#include <thread>
#include <condition_variable>
#include <sensor_msgs/Joy.h>

using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::string;
using std::vector;
using namespace std;
using namespace Eigen;

/**
 * @brief 检查电机速度是否安全（绝对值是否超过阈值）
 * @param jointd 电机速度向量（Eigen::VectorXf）
 * @param threshold 速度阈值（默认2.0）
 * @return true=安全，false=触发保护
 */
bool check_velocity_safe(const VectorXd &jointd, double threshold = 6.28f)
{
      for (int i = 0; i < jointd.size(); i++)
      {
            if (fabs(jointd(i)) > threshold)
            {
                  ROS_WARN("Motor %d velocity exceeds limit: %.3f", i + 1, jointd(i));
                  return false; // 触发保护
            }
      }
      return true; // 速度正常
}

/**
 * @brief 检查电机力矩是否安全（绝对值是否超过阈值）
 * @param tor 电机力矩
 * @param threshold 电机力矩阈值（默认2.0）
 * @return true=安全，false=触发保护
 */
bool check_torque_safe(const VectorXd &tor, double threshold = 6.28f)
{
      for (int i = 0; i < tor.size(); i++)
      {
            if (fabs(tor(i)) > threshold)
            {
                  ROS_WARN("Motor %d tor exceeds limit: %.3f", i + 1, tor(i));
                  return false; // 触发保护
            }
      }
      return true; // 速度正常
}

// 过渡
void now2start(float nowq[7], float startq[7], float qTrace[7][1000])
{
      float dltq[7] = {0}, stepq[7] = {0};
      for (size_t i = 0; i < 7; i++)
      {
            dltq[i] = startq[i] - nowq[i];
            stepq[i] = dltq[i] / 1000.0;
      }

      for (size_t i = 0; i < 1000; i++)
      {
            for (size_t motor_num = 0; motor_num < 7; motor_num++)
            {
                  qTrace[motor_num][i] = nowq[motor_num] + i * stepq[motor_num];
            }
      }
}

int main(int argc, char **argv)
{
      ros::init(argc, argv, "myARM_7dof");
      ros::NodeHandle n;
      ros::Rate r(500);
      lively_robot::robot rb; // 建一个机器人，包括板子，端口以及串口的线程打开等操作
      ROS_INFO("\033[1;32mSTART\033[0m");

      // 加载模型
      pinocchio::Model model;
      std::string model_path = "/home/koliko/ROS/arm_ws/src/urdf_7dof/urdf/urdf_7dof.urdf";
      pinocchio::urdf::buildModel(model_path, model);
      pinocchio::Data data(model);
      std::cout << "Model loaded with " << model.njoints << " joints." << std::endl;

      // 加载算法
      UR7DOFARM DofModel;

      // 获取电机数据
      motor_back_t joint_motor[7];

      // 各关节方向系数（1.0或-1.0）
      const double joint_direction[7] = {-1.0, -1.0, -1.0, 1.0, -1.0, -1.0, -1.0}; // 请根据实际情况修改

      // 安全检查
      // 获取电机当前状态
      // 初始化电机（7轴）
      uint8_t safe_check_flag = 1;
      if (safe_check_flag)
      {
            // 电机返回值（7个关节）
            double joint[7] = {0};
            for (size_t i = 0; i < 30; i++)
            {
                  for (int j = 0; j < 7; j++)
                  {
                        rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
                  }
                  rb.motor_send();
                  r.sleep();
            }
            // 获取所有关节状态
            for (int j = 0; j < 7; j++)
            {
                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                  joint[j] = joint_direction[j] * joint_motor[j].position;
            }

            // 检查所有关节的绝对值是否都小于0.2
            bool all_joints_safe = true;
            for (int j = 0; j < 7; j++)
            {
                  if (fabs(joint[j]) >= 0.2)
                  {
                        all_joints_safe = false;
                        break;
                  }
            }

            // 如果有关节不满足条件，跳出
            if (!all_joints_safe)
            {
                  ROS_ERROR("Motor position init error!");
                  // // 打印所有关节位置
                  ROS_INFO("Positions: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           joint[0], joint[1], joint[2], joint[3], joint[4], joint[5], joint[6]);
                  while (ros::ok())
                        ;
            }
      }

      // 获取电机模式
      std::string robot_mode;
      // 获取字符串参数
      if (n.getParam("/myARM_7dof/robot_mode", robot_mode))
      {
            ROS_INFO("Got robot mode: %s", robot_mode.c_str());
      }
      else
      {
            ROS_ERROR("Failed to get param '/myARM_7dof/robot_mode'");
      }

      // 正逆运动学
      if (robot_mode == "ik")
      {
            /*********过渡到期望位置*********************/
            float nowMotor[7] = {0}, startMotor[7] = {0};
            float mTrace[7][1000] = {0};

            startMotor[0] = 0.7;

            // 初始化电机（7轴）
            for (size_t k = 0; k < 30; k++)
            {
                  for (int j = 0; j < 7; j++)
                  {
                        rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
                  }
                  rb.motor_send();
                  r.sleep();
            }

            // 获取所有关节状态
            for (int j = 0; j < 7; j++)
            {
                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                  nowMotor[j] = joint_direction[j] * joint_motor[j].position;
            }

            now2start(nowMotor, startMotor, mTrace);
            cout << "》》》正在前往目的地！！\n"
                 << endl;

            for (size_t i = 0; i < 1000; i++)
            {
                  cout << "q:" << mTrace[0][i] << endl;
                  rb.Motors[0]->fresh_cmd(joint_direction[0] * mTrace[0][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[1]->fresh_cmd(joint_direction[1] * mTrace[1][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[2]->fresh_cmd(joint_direction[2] * mTrace[2][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[3]->fresh_cmd(joint_direction[3] * mTrace[3][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[4]->fresh_cmd(joint_direction[4] * mTrace[4][i], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[5]->fresh_cmd(joint_direction[5] * mTrace[5][i], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[6]->fresh_cmd(joint_direction[6] * mTrace[6][i], 0.0, 0, 200.0, 0.2); // q1
                  rb.motor_send();
                  r.sleep();
            }

            // 获取所有关节状态
            for (int j = 0; j < 7; j++)
            {
                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                  nowMotor[j] = joint_direction[j] * joint_motor[j].position;
            }
            startMotor[0] = 0;
            now2start(nowMotor, startMotor, mTrace);
            cout << "》》》正在前往目的地！！\n"
                 << endl;

            for (size_t i = 0; i < 1000; i++)
            {
                  cout << "q:" << mTrace[0][i] << endl;
                  rb.Motors[0]->fresh_cmd(joint_direction[0] * mTrace[0][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[1]->fresh_cmd(joint_direction[1] * mTrace[1][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[2]->fresh_cmd(joint_direction[2] * mTrace[2][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[3]->fresh_cmd(joint_direction[3] * mTrace[3][i], 0.0, 0, 500.0, 0.5); // q1
                  rb.Motors[4]->fresh_cmd(joint_direction[4] * mTrace[4][i], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[5]->fresh_cmd(joint_direction[5] * mTrace[5][i], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[6]->fresh_cmd(joint_direction[6] * mTrace[6][i], 0.0, 0, 200.0, 0.2); // q1
                  rb.motor_send();
                  r.sleep();
            }
      }

      /* 重力补偿和拖动示教 */
      uint32_t g_time = 10000; // ms
      if (robot_mode == "g")
      {
            cout << "正在进行7轴重力补偿测试\n"
                 << endl;

            // 使用 VectorXd 替代 double 数组
            VectorXd joint(7);
            VectorXd jointd(7);
            VectorXd jointdd(7);
            VectorXd tor(7);

            // 初始化电机（7轴）
            for (size_t i = 0; i < 30; i++)
            {
                  for (int j = 0; j < 7; j++)
                  {
                        rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
                  }
                  rb.motor_send();
                  r.sleep();
            }

            // 获取所有关节状态
            for (int j = 0; j < 7; j++)
            {
                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                  joint(j) = joint_direction[j] * joint_motor[j].position;
                  jointd(j) = joint_direction[j] * joint_motor[j].velocity;
                  jointdd(j) = 0; // 初始化为0
            }

            while (ros::ok())
            {
                  // 计算重力补偿力矩
                  tor = DofModel.Get_Tor(joint, jointd, jointdd, model, data);

                  // 发送补偿力矩（应用方向系数）
                  rb.Motors[0]->fresh_cmd(0, 0, tor(0), 0, 0);
                  rb.Motors[1]->fresh_cmd(0, 0, tor(1), 0, 0);
                  rb.Motors[2]->fresh_cmd(0, 0, tor(2), 0, 0);
                  rb.Motors[3]->fresh_cmd(0, 0, -tor(3), 0, 0);
                  rb.Motors[4]->fresh_cmd(0, 0, tor(4), 0, 0);
                  rb.Motors[5]->fresh_cmd(0, 0, tor(5), 0, 0);
                  rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[2]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[3]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[4]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[5]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);

                  rb.motor_send();

                  // 更新关节状态
                  for (int j = 0; j < 7; j++)
                  {
                        joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                        joint(j) = joint_direction[j] * joint_motor[j].position;
                        jointd(j) = joint_direction[j] * joint_motor[j].velocity;
                  }

                  // 电机保护
                  if (!check_velocity_safe(jointd))
                  {
                        for (size_t i = 0; i < 30; i++)
                        {
                              for (int j = 0; j < 7; j++)
                              {
                                    rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
                              }
                              rb.motor_send();
                              r.sleep();
                        }
                        while (ros::ok())
                              ;
                  }

                  ROS_INFO("Positions: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           joint(0), joint(1), joint(2), joint(3),
                           joint(4), joint(5), joint(6));
                  ROS_INFO("vel: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           jointd(0), jointd(1), jointd(2), jointd(3),
                           jointd(4), jointd(5), jointd(6));
                  ROS_INFO("TOR: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           tor(0), tor(1), tor(2), -tor(3),
                           tor(4), tor(5), tor(6));
                  r.sleep();
            }
            cout << "7轴测试完毕" << endl;
      }

      /* 七轴纯位置阻抗控制 + 重力补偿 */
      uint32_t ic_time = 20000; // 控制周期数
      if (robot_mode == "impedance_control")
      {
            // 1. 参数配置
            Eigen::Vector3d desired_pos(-0.3, -0.3, 0.09); // [m]

            // 关节状态变量
            Eigen::VectorXd current_joint_pos(7), current_joint_vel(7), joint_acc = Eigen::VectorXd::Zero(7);

            // 各向异性参数 (X/Y/Z方向独立设置)
            Eigen::Matrix3d Kp;
            Kp << 200.0, 0.0, 0.0, // X轴刚度 [N/m]
                0.0, 50.0, 0.0,    // Y轴刚度
                0.0, 0.0, 200.0;   // Z轴刚度
            Eigen::Matrix3d Kd = 2.0 * Kp.cwiseSqrt();

            // 2. 主控制循环
            while (ros::ok())
            {
                  // 获取当前关节状态
                  for (int j = 0; j < 7; j++)
                  {
                        joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                        current_joint_pos(j) = joint_direction[j] * joint_motor[j].position;
                        current_joint_vel(j) = joint_direction[j] * joint_motor[j].velocity;
                  }

                  // 3. 计算重力补偿力矩
                  Eigen::VectorXd gravity_torque = DofModel.Get_Tor(
                      current_joint_pos,
                      current_joint_vel,
                      joint_acc,
                      model,
                      data);

                  // 4. 计算纯位置阻抗力矩
                  Eigen::VectorXd impedance_torque = Eigen::VectorXd::Zero(7);
                  impedance_torque = DofModel.CalculatePositionImpedanceTorque(
                      current_joint_pos,
                      current_joint_vel,
                      desired_pos,
                      Kp,
                      Kd,
                      model,
                      data);

                  // 5. 合成总力矩
                  Eigen::VectorXd total_torque = -impedance_torque + gravity_torque;

                  // 6. 发送指令
                  rb.Motors[0]->fresh_cmd(0, 0, total_torque(0), 0, 0);
                  rb.Motors[1]->fresh_cmd(0, 0, total_torque(1), 0, 0);
                  rb.Motors[2]->fresh_cmd(0, 0, total_torque(2), 0, 0);
                  rb.Motors[3]->fresh_cmd(0, 0, -total_torque(3), 0, 0);
                  rb.Motors[4]->fresh_cmd(0, 0, total_torque(4), 0, 0);
                  rb.Motors[5]->fresh_cmd(0, 0, total_torque(5), 0, 0);
                  rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[2]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[3]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[4]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[5]->fresh_cmd(0, 0, 0, 0, 0);
                  // rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);

                  ROS_INFO("impedance_torque: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           impedance_torque(0), impedance_torque(1), impedance_torque(2), impedance_torque(3),
                           impedance_torque(4), impedance_torque(5), impedance_torque(6));

                  ROS_INFO("gravity_torque: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           gravity_torque(0), gravity_torque(1), gravity_torque(2), gravity_torque(3),
                           gravity_torque(4), gravity_torque(5), gravity_torque(6));
                  rb.motor_send();

                  r.sleep();
            }
      }

      // 获取电机当前状态
      while (ros::ok())
      {
            // 断电
            // rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0);
            rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);
            rb.Motors[2]->fresh_cmd(0, 0, 0, 0, 0);
            rb.Motors[3]->fresh_cmd(0, 0, 0, 0, 0);
            rb.Motors[4]->fresh_cmd(0, 0, 0, 0, 0);
            rb.Motors[5]->fresh_cmd(0, 0, 0, 0, 0);
            rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);
            rb.motor_send();

            joint_motor[0] = *rb.Motors[5]->get_current_motor_state();
            r.sleep();
            /* code */
      }

      // 释放
      for (auto &thread : rb.ser_recv_threads)
      {
            thread.join();
      }
}
